/*
 * main.c
 *
 *  Created on: Apr 15, 2013
 *      Author: Kurt Hardin
 */

#include <msp430.h> 

#include "constants.h"
#include "i2c.h"
#include "uart.h"

#define _get_high_hex_char(x)				HEX_CHARS[(unsigned char) ((x >> 4) & 0xF)]
#define _get_low_hex_char(x)				HEX_CHARS[(unsigned char) (x & 0xF)]
#define _print_mag_axis(label, idx)			uartSendByte(label), uartPutS(LABEL_HEX_PREFIX), uartSendByte(_get_high_hex_char(rxData[idx])), uartSendByte(_get_low_hex_char(rxData[idx])), uartSendByte(_get_high_hex_char(rxData[(unsigned char) (idx + 1)])), uartSendByte(_get_low_hex_char(rxData[(unsigned char) (idx + 1)])), uartPutS(LABEL_NLCR)


const char TxData[] = {
		0x00,
		0x70,
		0x20,
		0x00
};

const char RxAddr = 0x03;
char rxData[6];

int main(void) {
    WDTCTL = WDTPW | WDTHOLD;	// Stop watchdog timer

    // Set SMCLK to ~1MHz
    if (CALBC1_1MHZ==0xFF) {					// If calibration constant erased
    	while(1);                               // do not load, trap CPU!!
    }
    DCOCTL = 0;                               	// Select lowest DCOx and MODx settings
    BCSCTL1 = CALBC1_1MHZ;                    	// Set DCO
    DCOCTL = CALDCO_1MHZ;

    // Init USCIA0 for UART
    uartInit();

    // Init USCIB0 for I2C
    i2cInit(HMC5883_ADDR);

    // Init LED pin
    P1OUT &= ~BIT0;
    P1DIR |= BIT0;

    // Turn off unused ports
    P2OUT = 0x00;
    P2DIR = 0xFF;
    P3OUT = 0x00;
    P3DIR = 0xFF;

    // Configure HMC5883 magnetometer
    __delay_cycles(50000);     /* Wait ~50ms at DCO clock of ~1MHz */		// TODO Use timer
    i2cTransmit(&TxData[0], 4);

    while (1) {

    	// Wait for 'r' from UART
    	uartSetWaiting();
        __bis_SR_register(LPM0_bits + GIE);

        // Read magnetometer data
    	i2cReceiveFrom(RxAddr, &rxData[0], 6);

    	// Send mag data out UART
    	_print_mag_axis('X', 0);
    	_print_mag_axis('Y', 4);
    	_print_mag_axis('Z', 2);

    	// Flash LED on P1.0
    	P1OUT |= BIT0;              /* Turn on LED on P1.0 */
    	__delay_cycles(100000);     /* Wait ~100ms at DCO clock of ~1MHz */		// TODO Use timer
    	P1OUT &= ~BIT0;             /* Turn off LED on P1.0 */

    	// Send "DONE" message out UART
    	uartPutS(LABEL_DONE);
		uartPutS(LABEL_NLCR);
		uartPutS(LABEL_NLCR);
    }
}
